package com.hitqz.robot.arm.util;

import cn.hutool.core.net.NetUtil;
import cn.hutool.core.util.StrUtil;
import cn.hutool.json.JSONObject;
import cn.hutool.json.JSONUtil;
import com.hitqz.robot.arm.constant.RealManJointErrorEnum;
import com.hitqz.robot.arm.constant.RealManSysErrorEnum;
import com.hitqz.robot.camera.dto.RealManArmStateDto;
import lombok.Synchronized;
import lombok.extern.slf4j.Slf4j;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.net.ServerSocket;
import java.net.Socket;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;

/**
 * 瑞尔曼机械臂
 */
@Slf4j
public class RealManToolkit {

    private BufferedReader reader;
    private BufferedWriter writer;
    private Socket socket;
    private ServerSocket server;
    private final Integer port;
    private String ip;


    /**
     * 读取机械臂电源状态
     *
     * @return 1-上电 0-断电 -1-socket错误
     */
    public int getArmPowerState() {
        HashMap<String, String> map = new HashMap<>();
        map.put("command", "get_arm_power_state");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return -1;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        return jsonObject.getInt("power_state");
    }

    /**
     * 机械臂上电、断电
     *
     * @param cmd 1-上电 0-断电
     * @return true-成功 false-失败
     */
    public boolean setArmPower(int cmd) {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "set_arm_power");
        map.put("arm_power", cmd);
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("arm_power");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 机械臂初始化
     *
     * @return 1-上电 0-断电 -1-socket错误
     */
    public boolean setArmInt() {
        HashMap<String, String> map = new HashMap<>();
        map.put("command", "set_init_pose");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("init_pose");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 关节示教-关节拖动控制
     *
     * @param trajectoryRecord 拖动示教时记录轨迹，0-不记录，1-记录轨迹。
     * @return true-成功 false-失败
     */
    public boolean setDragTech(int trajectoryRecord) {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "start_drag_teach");
        map.put("trajectory_record", trajectoryRecord);
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("drag_teach");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }


    /**
     * 示教停止
     *
     * @return true-成功 false-失败
     */
    public boolean setStopTech() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "stop_drag_teach");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("drag_teach");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 获取机械臂状态
     *
     * @return RealManArmStateDto 机械臂状态类
     */
    public RealManArmStateDto getCurrentStatus() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "get_current_arm_state");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return null;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        RealManArmStateDto armState = jsonObject.get("arm_state", RealManArmStateDto.class);
        if (armState != null) {
            armState.setArmErrorString(RealManJointErrorEnum.JOINT_ERROR.get(armState.getArm_err()));
            armState.setSysErrorString(RealManSysErrorEnum.SYSTEM_ERROR.get(armState.getSys_err()));
            log.info("[RealMan] sys:{},joint:{}",armState.getSysErrorString() ,armState.getArmErrorString() );
        }
        return armState;
    }

    /**
     * 设置初始位置
     *
     * @param angle1 关节1角度
     * @param angle2 关节2角度
     * @param angle3 关节3角度
     * @param angle4 关节4角度
     * @param angle5 关节5角度
     * @param angle6 关节6角度
     * @param angle7 关节7角度
     * @return true-成功 false-失败
     */
    public boolean setInitPose(int angle1, int angle2, int angle3, int angle4, int angle5, int angle6, Integer angle7) {
        List<Integer> list = new ArrayList<>(7);
        list.add(angle1);
        list.add(angle2);
        list.add(angle3);
        list.add(angle4);
        list.add(angle5);
        list.add(angle6);
        if (null != angle7) {
            list.add(angle7);
        }
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "set_init_pose");
        map.put("init_pose", list);
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("init_pose");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 获取初始位置
     *
     * @return 关节1~6角度
     */
    public List<Integer> getInitPose() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "get_init_pose");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return null;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        return jsonObject.getJSONArray("init_pose").toList(Integer.class);
    }

    /**
     * 关节运动
     *
     * @param angle1 关节1角度
     * @param angle2 关节2角度
     * @param angle3 关节3角度
     * @param angle4 关节4角度
     * @param angle5 关节5角度
     * @param angle6 关节6角度
     * @param angle7 关节7角度
     * @param rate 速度百分比例系数，0~100
     * @return true-成功 false-失败
     */
    public boolean moveJoints(int angle1, int angle2, int angle3, int angle4, int angle5, int angle6, Integer angle7, int rate) {
        List<Integer> list = new ArrayList<>(7);
        list.add(angle1);
        list.add(angle2);
        list.add(angle3);
        list.add(angle4);
        list.add(angle5);
        list.add(angle6);
        if (null != angle7) {
            list.add(angle7);
        }
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "movej");
        map.put("joint", list);
        map.put("v", 30);//速度系数5%，0~100
        map.put("r", 0);//交融半径，暂不支持交融，默认 0
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("trajectory_state");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }


    /**
     * 轨迹急停
     *
     * @return true-成功 false-失败
     */
    public boolean setArmStop() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "set_arm_stop");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("arm_stop");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 轨迹暂停
     *
     * @return true-成功 false-失败
     */
    public boolean setArmPause() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "set_arm_pause");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("arm_pause");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 轨迹暂停后恢复
     *
     * @return true-成功 false-失败
     */
    public boolean setArmContinue() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "set_arm_continue");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("arm_continue");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }

    /**
     * 轨迹清除
     *
     * @return true-成功 false-失败
     */
    public boolean setDeleteCurrentTrajectory() {
        HashMap<String, Object> map = new HashMap<>();
        map.put("command", "set_delete_current_trajectory");
        String s = sendMessage(JSONUtil.toJsonStr(map));
        if (StrUtil.isBlank(s)) {
            return false;
        }
        JSONObject jsonObject = JSONUtil.parseObj(s);
        Boolean result = jsonObject.getBool("delete_current_trajectory");
        if (!result) {
            this.getCurrentStatus();
        }
        return result;
    }


    @Synchronized
    private String sendMessage(String data) {
        String ret = "";
        if (StrUtil.isNotBlank(data)) {
            try {
                //开socket
                if (StrUtil.isBlank(ip)) {
                    if (server == null) {
                        server = new ServerSocket(port);
                        //设置超时时间
                        server.setSoTimeout(20000);
                    }
                    socket = server.accept();
                } else {
                    socket = new Socket(ip, port);
                    socket.setSoTimeout(20000);
                }
                String clientAddr = socket.getRemoteSocketAddress().toString().substring(1);//需要跟下一句合并
                String[] clientAddrs = clientAddr.split(":");
                log.info("[RealMan] remote socket ip：{},port:{}", clientAddrs[0], clientAddrs[1]);
                reader = new BufferedReader(new InputStreamReader(socket.getInputStream()));
                writer = new BufferedWriter(new OutputStreamWriter(socket.getOutputStream()));
                //发消息
                writer.write(data);
                writer.flush();
                log.info("[RealMan] send:{}", data);
                //读数据
                ret = reader.readLine();
                ret = ret.replace("\r\n", "\\r\\n");
                log.info("[RealMan] reply：{}", ret);
                Thread.sleep(200);
            } catch (Exception e) {
                if (StrUtil.isNotBlank(ip)){
                    log.info("[RealMan]ping ip {} ,{}",ip,NetUtil.ping(ip));
                }
                log.error("[RealMan] socket send message error", e);
            } finally {
                try {
                    if (reader != null) reader.close();
                    if (writer != null) writer.close();
                    if (socket != null) socket.close();
                } catch (Exception e) {
                    log.error("");
                }
            }
        }
        return ret;
    }

    public RealManToolkit(int port) {
        this.port = port;
    }

    public RealManToolkit(String ip, int port) {
        this.port = port;
        this.ip = ip;
    }

    public static RealManToolkit asServer(int port) {
        return new RealManToolkit(port);
    }

    public static RealManToolkit asClient(String ip, int port) {
        return new RealManToolkit(ip, port);
    }


}
